#include "taskFlyport.h"
#include "MCP2515.h"
#include "CanDecode.h"
//ToDo
//Decode 1 or 2 messages that contain the necessary information about torque/drive mode/GSA etc.
//Transmit 1 message that contains information about drive mode/on off
//


Frame message[2];
Frame messageAct;
char buf[100];
int SOC = 0;
int ReAxlTorqActl = 0;
//int torqReqEmtf = 0;
int FrtAxlTorqActl = 0;
int GSApos = 0;
int DrvrReqMode = 0;
//int GrRat = 0;
//int CluInpTrq = 0;
int id;
BOOL debug = FALSE;
BOOL messageUseFlag = FALSE;

void FlyportTask()
{
	WFConnect(WF_DEFAULT);
	while (WFStatus != CONNECTED);
	UARTWrite(1,"Flyport connected... hello world!\r\n");
	
	// Initialise MCP2515 CAN controller at the specified speed and clock frequency
	// In this case 125kbps with a 16MHz oscillator
	// (Note:  This is the oscillator attached to the MCP2515, not the Arduino oscillaltor)
	SPIInit();

  int baudRate=Init_Simple(500,24);
	if(baudRate>0) {
		UARTWrite(1,"MCP2515 Init OK ...\r\n");		
		sprintf(buf,"Baud Rate (kbps): %i\n\r ",baudRate);
		UARTWrite(1,buf);
		//UARTWrite(1,baudRate,DEC);
	} 
	else {
		UARTWrite(1,"MCP2515 Init Failed ...\r\n");
	}
  //Add filter criteria below.
  if(Mode(MODE_CONFIG)){ //Put mcp2515 in configuration mode
  //Set up filters
  
  //Only allow message HMIrx on 0x1 on all incomming buffers
  
	Write_Simple(RXF0SIDH,0b00000000); //Only allow HMIrx through filter 0 on buffer 0. 0x1
	BitModify(RXF0SIDL,0b11100000,0b00100000); //Only allow HMIrx low side.
  
	Write_Simple(RXF1SIDH,0b00000000); //Only allow HMIrx through filter 1 on buffer 0x1
	BitModify(RXF1SIDL,0b11100000,0b00100000); //Only allow HMIrx low side.
  
	Write_Simple(RXM0SIDH,0xFF); //Set up mask on buffer 0 High side
	Write_Simple(RXM0SIDL,0xE0);//Mask buffer 0 Low side
	
	BitModify(RXB0CTRL, 0b00000100, 0b00100100); //enable filter on buffer 0 (RXF0, RXF1, RXM0) and set BUKT rollover flag
	//BitModify(RXB0CTRL, 0b01100100, 0b01100000); //enable filter on buffer 0 (RXF0, RXF1, RXM0) and set BUKT rollover flag

  
  //Set up filter 1 266,264,161
	
	Write_Simple(RXF2SIDH,0b00000000); //Only allow HMIrx through filter 2 on buffer 1. 0x1
	BitModify(RXF2SIDL,0b11100000,0b00100000); //Only allow VCU_7(ReAxlTorqReq) low side.
	
	Write_Simple(RXF3SIDH,0b00000000); //Only allow HMIrx through filter 3 on buffer 1. 0x1
	BitModify(RXF3SIDL,0b11100000,0b00100000); //Only allow HMIrx low side.
	
	Write_Simple(RXF4SIDH,0b00000000); //Only allow TCU_2(DCTTqRat) through filter4 on buffer 1. 0x1
	BitModify(RXF4SIDL,0b11100000,0b00100000); //Only allow TCU_2(DCTTqRat) low side.
  
	Write_Simple(RXM1SIDH,0xFF); //Set up mask on buffer 0 High side
	Write_Simple(RXM1SIDL,0xE0);//Mask buffer 0 Low side
  
	BitModify(RXB1CTRL, 0b01100000, 0b00100000); //enable filter on buffer 1
  	//BitModify(RXB1CTRL, 0b01100000, 0b01100000); //enable filter on buffer 1

	  }
	
	if(Mode(MODE_LISTEN)){ //Put in Listen only mode.
		UARTWrite(1,"Listen Only Mode\r\n");
			}
		UARTWrite(1,"Ready ...\r\n");
	
		
	if (!debug) {
		UARTWrite(1,"Debug Mode: OFF\r\n");
		
	while(1){
				  
	// This implementation utilizes the MCP2515 INT pin to flag received messages or other events
	if(MCP_Interrupt()) {
    // determine which interrupt flags have been set
    BYTE interruptFlags = Read_Simple(CANINTF);
    
		if(interruptFlags & ERRIF){
	
			//Read EFLG register to find out which error is set.
			BYTE errorState = Read_Simple(EFLG);
			//sprintf(buf, "Error: %d \n\r", errorState);
			//UARTWrite(1,buf);
			BitModify(CANINTF,0b00100000,0b00000000);//clear error interrupt
			BitModify(EFLG,0b11000000,0b00000000); //Clear errors
			//64 = Recieve buffer 0 overflow
			}
			
		if((interruptFlags & RX0IF)) {
			// read from RX buffer 0
			message[1] = ReadBuffer(RXB0);	
			//sprintf(buf, "First interrupt");
			//UARTWrite(1,buf);
			}
	
		if((interruptFlags & RX1IF)>>1) { 
			// read from RX buffer 1
			message[2] = ReadBuffer(RXB1);
			}
	}
  
	// Selection of signals within selected messages to be sent out to webpage	
	//sprintf(buf, "ID: %d \n\r", id);
	//	UARTWrite(1,buf);

	   BYTE MSB = 0;
	   BYTE LSB = 0;
	   
	if(id == 1){ //HMIrx
		//In this method, the signals will be decoded from the incomming bits and converted into physical values. See soc example below
		
		//GSApos: Byte 0, bits 3-4
		//DrvrReqMode: Byte 0, bits 5-7
		//FrtAxlTorqActl: Byte 1, bits 8-15
		//ReAxlTorqActl: Byte 2, bits 16-23
		//SOC: Byte 3, bits 24-31
		
		GSApos = message[1].data[0] & 0b00011000;
		DrvrReqMode = message[1].data[0] & 0b00000111;
		FrtAxlTorqActl = message[1].data[1];
		ReAxlTorqActl = message[1].data[2];
		SOC = message[1].data[3];
		
		//***Example***
		//Soc is represented with bits 16-23 and 30-31 shall be bit 30-31+16-23 
		//soc = MSBLSB(message[1].data[3] & 0b00000011,message[1].data[2],8) * 0.1;
		//**/Example**
		 
		//sprintf(buf, "Case SOC");
		//UARTWrite(1,buf);
	}
}	
}

	else 
		UARTWrite(1,"Debug mode: ON \r\n");
		while(1){
			DelayMs(50);
			if(SOC >= 100){
				SOC = 0;
				ReAxlTorqActl = -100;
				FrtAxlTorqActl = -100;
				DrvrReqMode = -100;
				GSApos = 0;
				}
			SOC = SOC + 1;
			ReAxlTorqActl = ReAxlTorqActl + 2;
			FrtAxlTorqActl = FrtAxlTorqActl + 2;
			GSApos = GSApos +1;
			//sprintf(buf, "State of Charge: %d \n\r", soc);
			//UARTWrite(1,buf);
}
}
	
